#include "pid.h"

static float clampf(float v, float lo, float hi)
{
  if (v < lo) return lo;
  if (v > hi) return hi;
  return v;
}

void PID_Init(PIDController* pid,
              float kp,
              float ki,
              float kd,
              float out_min,
              float out_max,
              float integrator_min,
              float integrator_max)
{
  pid->kp = kp;
  pid->ki = ki;
  pid->kd = kd;

  pid->out_min = out_min;
  pid->out_max = out_max;

  pid->integrator_min = integrator_min;
  pid->integrator_max = integrator_max;

  pid->integral = 0.0f;
  pid->prev_error = 0.0f;

  // Smoothing defaults disabled
  pid->meas_filtered = 0.0f;
  pid->meas_tau = 0.0f;
  pid->last_output = 0.0f;
  pid->out_tau = 0.0f;
  pid->max_slew_per_sec = 0.0f;
  pid->meas_filter_initialized = 0u;
  pid->output_initialized = 0u;
}

void PID_Reset(PIDController* pid)
{
  pid->integral = 0.0f;
  pid->prev_error = 0.0f;
  pid->meas_filtered = 0.0f;
  pid->last_output = 0.0f;
  pid->meas_filter_initialized = 0u;
  pid->output_initialized = 0u;
}

float PID_Compute(PIDController* pid,
                  float setpoint,
                  float measurement,
                  float dt_seconds)
{
  if (dt_seconds <= 0.0f) dt_seconds = 1e-3f;

  // Optional measurement low-pass filter
  float meas_used = measurement;
  if (pid->meas_tau > 0.0f)
  {
    if (!pid->meas_filter_initialized)
    {
      pid->meas_filtered = measurement;
      pid->meas_filter_initialized = 1u;
    }
    float alpha_m = dt_seconds / (pid->meas_tau + dt_seconds);
    pid->meas_filtered = pid->meas_filtered + alpha_m * (measurement - pid->meas_filtered);
    meas_used = pid->meas_filtered;
  }

  float error = setpoint - meas_used;

  // Integrate with clamping to prevent windup
  pid->integral += error * dt_seconds;
  pid->integral = clampf(pid->integral, pid->integrator_min, pid->integrator_max);

  // Derivative (on measurement would be: -(measurement - prev_measurement)/dt)
  float derivative = (error - pid->prev_error) / dt_seconds;

  float output_raw = pid->kp * error + pid->ki * pid->integral + pid->kd * derivative;

  // Optional output smoothing (low-pass) and slew-rate limiting
  float output_candidate = output_raw;

  if (pid->out_tau > 0.0f)
  {
    if (!pid->output_initialized)
    {
      pid->last_output = output_raw;
      pid->output_initialized = 1u;
    }
    float alpha_o = dt_seconds / (pid->out_tau + dt_seconds);
    output_candidate = pid->last_output + alpha_o * (output_raw - pid->last_output);
  }

  if (pid->max_slew_per_sec > 0.0f)
  {
    float max_delta = pid->max_slew_per_sec * dt_seconds;
    float delta = output_candidate - pid->last_output;
    if (delta > max_delta)       output_candidate = pid->last_output + max_delta;
    else if (delta < -max_delta) output_candidate = pid->last_output - max_delta;
  }

  float output = clampf(output_candidate, pid->out_min, pid->out_max);

  pid->prev_error = error;
  pid->last_output = output;
  return output;
}

void PID_ConfigFilter(PIDController* pid,
                      float meas_tau_seconds,
                      float out_tau_seconds)
{
  pid->meas_tau = (meas_tau_seconds > 0.0f) ? meas_tau_seconds : 0.0f;
  pid->out_tau  = (out_tau_seconds  > 0.0f) ? out_tau_seconds  : 0.0f;
  pid->meas_filter_initialized = 0u;
  pid->output_initialized = 0u;
}

void PID_ConfigSlew(PIDController* pid, float max_rate_per_sec)
{
  pid->max_slew_per_sec = (max_rate_per_sec > 0.0f) ? max_rate_per_sec : 0.0f;
}


